
#include "Elem_Shell_Flat_QUAD4.h"

#include "MyTime.h"
#include "BasisFunctionsLagrange.h"
#include "GeomDataLagrange.h"
#include "QuadratureUtil.h"
#include "TimeFunction.h"
#include "util.h"


extern  std::vector<unique_ptr<TimeFunction> > timeFunction;
extern  MyTime           myTime;
extern  bool  debug;

using namespace std;


Elem_Shell_Flat_QUAD4::Elem_Shell_Flat_QUAD4()
{
  ndof   = 6;
  ndim   = 3;
  npElem = 4;
  nsize  = npElem*ndof;

  if (debug) cout << " constructor Elem_Shell_Flat_QUAD4\n\n";
}



Elem_Shell_Flat_QUAD4::~Elem_Shell_Flat_QUAD4()
{
  if (debug) cout << " destructor Elem_Shell_Flat_QUAD4\n\n";
}



void Elem_Shell_Flat_QUAD4::prepareElemData()
{
  ElementBase::prepareElemData();

  return;
}



int Elem_Shell_Flat_QUAD4::calcLoadVector(VectorXd& Flocal)
{
  return 0;
}




int Elem_Shell_Flat_QUAD4::calcStiffnessAndResidual(MatrixXd& Klocal, VectorXd& Flocal)
{
    int  ii, jj, gp, kk, ind1, ind2, r1, r2, r3, r4, r5, r6;

    double  fact, fact1, fact2, dvol, param[2];
    double  Jac, bb1, bb2, bb3, xNode[npElem], yNode[npElem];

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);
    VectorXd  dispC(nsize), velC(nsize), accC(nsize), stre(3);
    MatrixXd  Mlocal(nsize, nsize), Dm(3,3), Db(3,3), Ds(2,2), Dmat(6,6), NN(3,nsize), Bmat(6,nsize);
    NN.setZero();
    Bmat.setZero();
    MatrixXd  grad(3,3), dphis(3,3), RotMat(3,3), RotMatFull(nsize,nsize);
    VectorXd  phis(3), strains(6);

    myPoint  bforce = GeomData->getBodyForce();
    int bftf = GeomData->getBodyForceTimeFunction();
    if( bftf >= 0)
      bforce *= timeFunction[bftf]->getFactor() ;

    double rho0  = MatlData->getDensity() ;
    double rho   = rho0;

    double       af = SolnData->td(2);
    double acceFact = SolnData->td(5);
    double veloFact = SolnData->td(10);

    vector<double> matDat = MatlData->getData();
    vector<double> elmDat = ElemTypeData->getData();

    double  E     = matDat[0];
    double  nu    = matDat[1];

    double  h     = elmDat[0];
    double  kappa = 5.0/6.0;

    double  G     = E/(2.0*(1.0+nu));
    double  I     = h*h*h/12.0;

    // Dm - elasticity matrix for the membrane part
    Dm.setZero();
    Dm(0,0) = 1.0;   Dm(0,1) = nu;
    Dm(1,0) = nu;    Dm(1,1) = 1.0;
    Dm(2,2) = (1.0-nu)*0.5;

    Dm = (E*h/(1.0-nu*nu))*Dm;

    // Db - elasticity matrix for the bending part
    Db = (h*h/12.0)*Dm;

    // Ds - elasticity matrix for the shear part
    Ds.setZero();
    Ds(0,0) = G;
    Ds(1,1) = G;
    Ds = (h*kappa)*Ds;

    // Combined D matrix for membrane and bending parts
    Dmat.setZero();

    Dmat.block<3,3>(0,0) = Dm;
    Dmat.block<3,3>(3,3) = Db;


    vector<myPoint> coordsGlobal(npElem);
    myPoint  pt1, pt2;

    for(ii=0; ii<npElem; ii++)
    {
      coordsGlobal[ii][0] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      coordsGlobal[ii][1] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
      coordsGlobal[ii][2] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][2];
    }

    compute_transformations_flat_shell(coordsGlobal, RotMat);

    // transform body force to local coordinates
    bforce = RotMat*bforce;

    RotMatFull.setZero();
    for(ii=0; ii<2*npElem; ii++)
    {
      ind1 = 3*ii;
      RotMatFull.block<3,3>(ind1,ind1) = RotMat;
    }

    // transform nodal coordinates into local system
    for(ii=0; ii<npElem; ii++)
    {
      pt1 = RotMat*(coordsGlobal[ii]-coordsGlobal[0]);

      xNode[ii] = pt1[0];
      yNode[ii] = pt1[1];
    }


    VectorXd solnElemGlobal(nsize);
    VectorXd solnElemLocal(nsize);

    // transform nodal solution into local system
    for(ii=0; ii<npElem; ii++)
    {
      ind1 = ndof*ii;
      ind2 = nodeNums[ii]*ndof;

      //for(kk=0;kk<ndof;kk++)
      //{
        //solnElemGlobal(ind1+kk)  =  SolnData->dispCur[ind2+kk];
        //velC(ind1+kk)   =  SolnData->veloCur[ind2+kk];
        //accC(ind1+kk)   =  SolnData->acceCur[ind2+kk];
      //}

      // displacements
      pt1[0] = SolnData->dispCur[ind2];
      pt1[1] = SolnData->dispCur[ind2+1];
      pt1[2] = SolnData->dispCur[ind2+2];
      pt1 = RotMat*pt1;

      solnElemLocal[ind1]   = pt1[0];
      solnElemLocal[ind1+1] = pt1[1];
      solnElemLocal[ind1+2] = pt1[2];

      // rotations
      pt2[0] = SolnData->dispCur[ind2+3];
      pt2[1] = SolnData->dispCur[ind2+4];
      pt2[2] = SolnData->dispCur[ind2+5];
      pt2 = RotMat*pt2;

      solnElemLocal[ind1+3] = pt2[0];
      solnElemLocal[ind1+4] = pt2[1];
      solnElemLocal[ind1+5] = pt2[2];
    }

    //printMatrix(coordsGlobal);
    //printMatrix(RotMat);
    //printMatrix(coordsLocal);

    if(Klocal.rows() != nsize)
    {
      Klocal.resize(nsize, nsize);
      Flocal.resize(nsize);
    }
    Klocal.setZero();
    Flocal.setZero();
    Mlocal.setZero();

    //   part 1. -- contribution due to bending

    int nGP;
    vector<double>  gausspoints1, gausspoints2, gaussweights;
    
    if(npElem == 6)
    {
      nGP = 3;
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    if(npElem == 4)
    {
      nGP = 4;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    if(npElem == 9)
    {
      nGP = 9;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }

    for(gp=0;gp<nGP;gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        shape_functions_Derivatives_Shell(ELEM_SHAPE_QUAD, npElem, param, xNode, yNode, &N(0), &dN_dx(0), &dN_dy(0), Jac);
        dvol = gaussweights[gp] * Jac;

        grad.setZero();
        phis.setZero();
        dphis.setZero();
        strains.setZero();
        Bmat.setZero();

        for(ii=0; ii<npElem; ii++)
        {
          bb1 = dN_dx[ii];
          bb2 = dN_dy[ii];
          bb3 = N[ii];
    
          r1 = ndof*ii;
          r2 = r1+1;
          r3 = r1+2;
          r4 = r1+3;
          r5 = r1+4;
          r6 = r1+5;

          grad(0,0)  += solnElemLocal[r1] * bb1;
          grad(0,1)  += solnElemLocal[r1] * bb2;
          grad(1,0)  += solnElemLocal[r2] * bb1;
          grad(1,1)  += solnElemLocal[r2] * bb2;
          grad(2,0)  += solnElemLocal[r3] * bb1;
          grad(2,1)  += solnElemLocal[r3] * bb2;

          phis(0)    += solnElemLocal[r4] * bb3;
          phis(1)    += solnElemLocal[r5] * bb3;
          phis(2)    += solnElemLocal[r6] * bb3;
    
          dphis(0,0) += solnElemLocal[r4] * bb1;
          dphis(0,1) += solnElemLocal[r4] * bb2;
          dphis(1,0) += solnElemLocal[r5] * bb1;
          dphis(1,1) += solnElemLocal[r5] * bb2;
          dphis(2,0) += solnElemLocal[r6] * bb1;
          dphis(2,1) += solnElemLocal[r6] * bb2;

          // membrane part
          Bmat(0,r1) = bb1;
          Bmat(1,r2) = bb2;
          Bmat(2,r1) = bb2;     Bmat(2,r2) = bb1;

          // bending part
          Bmat(3,r5) = bb1;
          Bmat(4,r4) = bb2;
          Bmat(5,r4) = bb1;     Bmat(5,r5) = bb2;

          NN(0,r1) = bb3;
          NN(1,r2) = bb3;
          NN(2,r3) = bb3;

          bb3 = bb3*dvol;

          Flocal(r1) += (bb3*bforce[0]) ;
          Flocal(r2) += (bb3*bforce[1]) ;
          Flocal(r3) += (bb3*bforce[2]) ;
        }

        // Bm - membrane part
        strains[0] = grad(0,0);
        strains[1] = grad(1,1);
        strains[2] = grad(0,1) + grad(1,0);

        // Bb - bending part
        strains[3] = dphis(1,0);
        strains[4] = dphis(0,1);
        strains[5] = dphis(0,0) + dphis(1,1);

        stre = Dmat*strains;

        Klocal += ( Bmat.transpose()* ((Dmat*af*dvol)*Bmat) );
        Flocal -= ( Bmat.transpose()*(stre*dvol));

        //Mlocal += (NN.transpose()*NN*dvol);
    }//gp

    //printMatrix(Klocal);  printf("\n\n\n");  printVector(Flocal);

    //   part 2. -- contribution due to shear

    if(npElem == 6)
    {
      nGP = 1;
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    if(npElem == 4)
    {
      nGP = 1;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    if(npElem == 9)
    {
      nGP = 4;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }


    Bmat.resize(2,nsize);
    strains.resize(2);

    for(gp=0;gp<nGP;gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        shape_functions_Derivatives_Shell(ELEM_SHAPE_QUAD, npElem, param, xNode, yNode, &N(0), &dN_dx(0), &dN_dy(0), Jac);
        dvol = gaussweights[gp] * Jac;

        grad.setZero();
        phis.setZero();
        strains.setZero();
        Bmat.setZero();

        for(ii=0;ii<npElem;ii++)
        {
          bb1 = dN_dx[ii];
          bb2 = dN_dy[ii];
          bb3 = N[ii];

          r1 = ndof*ii;
          r2 = r1+1;
          r3 = r1+2;
          r4 = r1+3;
          r5 = r1+4;
          r6 = r1+5;

          grad(2,0) += solnElemLocal[r3] * bb1;
          grad(2,1) += solnElemLocal[r3] * bb2;

          phis(0)   += solnElemLocal[r4] * bb3;
          phis(1)   += solnElemLocal[r5] * bb3;
          phis(2)   += solnElemLocal[r6] * bb3;

          // shear part
          Bmat(0,r3) = bb1;     Bmat(0,r5) = -bb3;
          Bmat(1,r3) = bb2;     Bmat(1,r4) = -bb3;
        }

        // Bs - shear part
        strains[0] = grad(2,0) - phis[1];
        strains[1] = grad(2,1) - phis[0];

        stre = Ds*strains;

        Klocal += (Bmat.transpose()*((Ds*af*dvol)*Bmat) );
        Flocal -= (Bmat.transpose()*(stre*dvol));

    }//gp

    // adjust the stiffness matrix for the sixth DOF
    //
    fact = -1.0;
    for(ii=0; ii<nsize; ii++)
      fact = max( fact, abs(Klocal(ii,ii)) );

    fact /= 1000.0;
    for(ii=0; ii<npElem; ii++)
    {
      r6 = ndof*ii+5;

      Klocal(r6, r6) += fact;
      Flocal(r6)     += fact*(0.0-solnElemLocal[r6]);
    }


    // Transform stiffness matrix and force vector to global coordinate system
    Klocal = RotMatFull.transpose()*Klocal*RotMatFull;
    Flocal = RotMatFull.transpose()*Flocal;

    //printMatrix(Klocal);  printf("\n\n\n");  printVector(Flocal);

    return 0;
}





int Elem_Shell_Flat_QUAD4::calcInternalForces()
{
  return 0;
}



void Elem_Shell_Flat_QUAD4::elementContourplot(int vartype, int varindex, int index)
{
  return;
}


void Elem_Shell_Flat_QUAD4::projectToNodes(bool extrapolateFlag, int vartype, int varindex, int index)
{
  return;
}


void Elem_Shell_Flat_QUAD4::projectStress(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}


void Elem_Shell_Flat_QUAD4::projectStrain(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}



void Elem_Shell_Flat_QUAD4::projectInternalVariable(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
   return;
}

